{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridJunctionTree"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridJunctionTree.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "A `HybridJunctionTree` is the hybrid equivalent of `gtsam.GaussianJunctionTree`. It represents an intermediate stage in **multifrontal** variable elimination on a `HybridGaussianFactorGraph`.\n",
        "\n",
        "It is a tree structure where each node (a \"cluster\" or \"clique\") contains:\n",
        "1.  A set of frontal variables (those being eliminated at this node).\n",
        "2.  A set of separator variables (those shared with the parent clique).\n",
        "3.  The original factors from the `HybridGaussianFactorGraph` that involve *only* the frontal and separator variables of this clique and its descendants in the elimination tree.\n",
        "\n",
        "Key differences:\n",
        "*   **vs. HybridEliminationTree:** A junction tree node can eliminate multiple variables (frontals) at once, whereas an elimination tree node corresponds to a single variable elimination.\n",
        "*   **vs. HybridBayesTree:** A junction tree node stores the *original factors* before elimination, while a Bayes tree node stores the *result* of eliminating those factors (a `HybridConditional`).\n",
        "\n",
        "Like `HybridEliminationTree`, the `HybridJunctionTree` is primarily an internal data structure used by the `eliminateMultifrontal` process. Its structure directly informs the structure of the resulting `HybridBayesTree`. Direct manipulation in Python is uncommon."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "from gtsam import (\n",
        "    HybridGaussianFactorGraph,\n",
        "    Ordering,\n",
        "    HybridEliminationTree,\n",
        "    HybridJunctionTree,\n",
        "    JacobianFactor,\n",
        "    DecisionTreeFactor,\n",
        "    HybridGaussianFactor,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Creating a HybridJunctionTree\n",
        "\n",
        "It is constructed from a `HybridEliminationTree`. The process typically involves:\n",
        "1. Create `HybridGaussianFactorGraph`.\n",
        "2. Determine an `Ordering`.\n",
        "3. Build the `HybridEliminationTree`.\n",
        "4. Build the `HybridJunctionTree` from the `HybridEliminationTree`.\n",
        "\n",
        "This process is encapsulated within `HybridGaussianFactorGraph.eliminateMultifrontal`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original HybridGaussianFactorGraph:\n",
            "\n",
            "size: 4\n",
            "Factor 0\n",
            "DiscreteFactor:\n",
            " f[ (d0,2), ]\n",
            " Choice(d0) \n",
            " 0 Leaf  0.6\n",
            " 1 Leaf  0.4\n",
            "\n",
            "Factor 1\n",
            "GaussianFactor:\n",
            "\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "\n",
            "Factor 2\n",
            "HybridGaussianFactor:\n",
            "Hybrid [x1; d0]{\n",
            " Choice(d0) \n",
            " 0 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.5\n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 5 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "\n",
            "Factor 3\n",
            "GaussianFactor:\n",
            "\n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.316228\n",
            "\n",
            "\n",
            "Elimination Ordering: Position 0: x0, x1, d0\n",
            "\n",
            "\n",
            "HybridEliminationTree:\n",
            "-(d0)\n",
            "- f[ (d0,2), ]\n",
            " Choice(d0) \n",
            " 0 Leaf  0.6\n",
            " 1 Leaf  0.4\n",
            "| -(x1)\n",
            "| -\n",
            "Hybrid [x1; d0]{\n",
            " Choice(d0) \n",
            " 0 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.5\n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 5 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "| | -(x0)\n",
            "| | -\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "| | -\n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.316228\n",
            "\n",
            "Resulting HybridJunctionTree:\n",
            "- (4) d0 \n",
            "| - (4) x1 \n",
            "| | - (4) x0 \n"
          ]
        }
      ],
      "source": [
        "# --- Create a HybridGaussianFactorGraph (same as HBT/HET examples) ---\n",
        "hgfg = HybridGaussianFactorGraph()\n",
        "dk0 = (D(0), 2)\n",
        "prior_d0 = DecisionTreeFactor([dk0], \"0.6 0.4\"); hgfg.add(prior_d0) # F0\n",
        "prior_x0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0)); hgfg.add(prior_x0) # F1\n",
        "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n",
        "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n",
        "meas_x1_d0 = HybridGaussianFactor(dk0, [gf0, gf1]); hgfg.add(meas_x1_d0) # F2\n",
        "odom_x0_x1 = JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1))); hgfg.add(odom_x0_x1) # F3\n",
        "print(\"Original HybridGaussianFactorGraph:\")\n",
        "hgfg.print()\n",
        "\n",
        "# --- Ordering and Elimination Tree ---\n",
        "ordering = gtsam.Ordering([X(0), X(1), D(0)])\n",
        "print(f\"\\nElimination Ordering: {ordering}\")\n",
        "het = HybridEliminationTree(hgfg, ordering)\n",
        "print(\"\\nHybridEliminationTree:\")\n",
        "het.print()\n",
        "\n",
        "# --- Construct the Junction Tree ---\n",
        "hjt = HybridJunctionTree(het)\n",
        "print(\"\\nResulting HybridJunctionTree:\")\n",
        "# Printing shows cliques, separators, and the *original factor indices* stored within each clique.\n",
        "hjt.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Elimination\n",
        "\n",
        "The primary purpose of the junction tree is to structure the multifrontal elimination process, which ultimately yields a `HybridBayesTree`. The `eliminate` method of the junction tree performs this step, but it's usually accessed via `HybridGaussianFactorGraph.eliminateMultifrontal`."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
